#include <stdio.h>
#include <stdlib.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"

#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10
#define GPIOFUNC 0

#define DUTY_100_PERCENT 100             // 100% 占空比
#define DUTY_70_PERCENT 700              // 70% 占空比
#define DUTY_50_PERCENT 500              // 50% 占空比
#define DUTY_30_PERCENT 300              // 30% 占空比
#define DUTY_0_PERCENT 0              // 0% 占空比

// 新增 PWM 端口定义，假设使用 PWM0 和 PWM1 控制左右电机
#define PWM_PORT_LEFT HI_PWM_PORT_PWM4
#define PWM_PORT_RIGHT HI_PWM_PORT_PWM1  
#define PWM_FREQ 1000                    // PWM频率1KHz (周期1ms)

#define FULL_SPEED 0
#define FAST_SPEED 300
#define MID_SPEED 500
#define SLOW_SPEED 700
#define STOP_SPEED 1000

int speed_mode[5] = {
    FULL_SPEED,
    FAST_SPEED,
    MID_SPEED,
    SLOW_SPEED,
    STOP_SPEED,
};

int speed_mode_index = 0;
int global_speed = FULL_SPEED;

void switch_speed(){
    speed_mode_index++;
    if (speed_mode_index >= sizeof(speed_mode)/sizeof(speed_mode[0])) {
        speed_mode_index = 0;
    }
    global_speed = speed_mode[speed_mode_index];
}

typedef enum CarDirection {
    DIR_FORWARD = 1,
    DIR_BACKWARD = 2,
    DIR_LEFT = 3,
    DIR_RIGHT = 4,
    DIR_STOP = 5,
}CarDir;
CarDir car_dir_index = DIR_STOP;
void car_run(CarDir direction){
    if(direction == NULL){
        direction = car_dir_index;
    }
    switch (direction)
    {
    case DIR_FORWARD:car_forward();break;
    case DIR_BACKWARD:car_backward();break;
    case DIR_LEFT:car_left();break;
    case DIR_RIGHT:car_right();break;
    case DIR_STOP:car_stop();break;
    default:car_stop();break;
    }
}

void get_car_direction(){
    hi_u8 gpio0_func,gpio1_func,gpio9_func,gpio10_func;
    hi_io_get_func(GPIO0,&gpio0_func);
    hi_io_get_func(GPIO1,&gpio1_func);
    hi_io_get_func(GPIO9,&gpio9_func);
    hi_io_get_func(GPIO10,&gpio10_func);
    int judgeDir = 0;
    if(gpio0_func==GPIOFUNC) judgeDir+=8;
    if(gpio1_func==GPIOFUNC) judgeDir+=4;
    if(gpio9_func==GPIOFUNC) judgeDir+=2;
    if(gpio10_func==GPIOFUNC) judgeDir+=1;

    if(judgeDir==0b1111){
        car_dir_index = DIR_STOP;
    }
    else if(judgeDir==0b1000){
        car_dir_index = DIR_RIGHT;
    }
    else if(judgeDir==0b0010){
        car_dir_index = DIR_LEFT;
    }
    else if(judgeDir==0b1010){
        car_dir_index = DIR_FORWARD;
    }
    else if(judgeDir==0b0101){
        car_dir_index = DIR_BACKWARD;
    }
}

struct Gpio2PWM{
    unsigned int pwm_out;
    unsigned int pwm_port;
};

struct Gpio2PWM GS[11]={
    {HI_IO_FUNC_GPIO_0_PWM3_OUT,HI_PWM_PORT_PWM3},
    {HI_IO_FUNC_GPIO_1_PWM4_OUT,HI_PWM_PORT_PWM4},
    {HI_IO_FUNC_GPIO_2_PWM2_OUT,HI_PWM_PORT_PWM2},
    {HI_IO_FUNC_GPIO_3_PWM5_OUT,HI_PWM_PORT_PWM5},
    {HI_IO_FUNC_GPIO_4_PWM1_OUT,HI_PWM_PORT_PWM1},
    {HI_IO_FUNC_GPIO_5_PWM2_OUT,HI_PWM_PORT_PWM2},
    {HI_IO_FUNC_GPIO_6_PWM3_OUT,HI_PWM_PORT_PWM3},
    {HI_IO_FUNC_GPIO_7_PWM0_OUT,HI_PWM_PORT_PWM0},
    {HI_IO_FUNC_GPIO_8_PWM1_OUT,HI_PWM_PORT_PWM1},
    {HI_IO_FUNC_GPIO_9_PWM0_OUT,HI_PWM_PORT_PWM0},
    {HI_IO_FUNC_GPIO_10_PWM1_OUT,HI_PWM_PORT_PWM1},
};


// 通过查询机器人板硬件原理图可知，机器人板上电机驱动芯片使用的是L9110S
// 此器件能够驱动一个直流无刷电机，集成了电机正转/反转/停止/刹车四个功能。
// 左右两轮电机各由一个L9110S驱动,GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车

// gpio_control用于控制GPIO引脚的输出电平值
void gpio_control (unsigned int  gpio, IotGpioValue value) {
    if(value==IOT_GPIO_VALUE0){
        hi_io_set_func(gpio, GS[gpio].pwm_out);
        IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);//将GPIO引脚设置为输出方向，表示该引脚将用作输出信号的发送端。在输出模式下，可以通过控制GPIO引脚的输出电平值
        hi_pwm_init(GS[gpio].pwm_port);
        hi_pwm_set_clock(PWM_CLK_160M);
        hi_pwm_start(GS[gpio].pwm_port, global_speed, PWM_FREQ);
    }
    else{
        hi_io_set_func(gpio, GPIOFUNC);
        IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);//将GPIO引脚设置为输出方向，表示该引脚将用作输出信号的发送端。在输出模式下，可以通过控制GPIO引脚的输出电平值
        IoTGpioSetOutputVal(gpio, value);//设置GPIO引脚的输出电平值。
    }
}

// 小车后退
// GPIO0为低电平，GPIO1为高电平，左轮反转
// GPIO9为低电平，GPIO10为高电平，右轮反转
void car_backward(void) {
    car_dir_index = DIR_BACKWARD;
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    gpio_control(GPIO1, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}

// 小车前进
// GPIO0为高电平，GPIO1为低电平，左轮正转
// GPIO9为高电平，GPIO10为低电平，右轮正转
void car_forward(void) {
    car_dir_index = DIR_FORWARD;
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}

// 小车左转
// GPIO0和1为低电平,左轮停止
// GPIO9为高电平，GPIO10为低电平，右轮正转
void car_left(void) {
    car_dir_index = DIR_LEFT;
    gpio_control(GPIO0, IOT_GPIO_VALUE0);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}


// 小车右转
// GPIO0为高电平，GPIO1为低电平，左轮正转
// GPIO9和10为低电平,右轮停止
void car_right(void) {
    car_dir_index = DIR_RIGHT;
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE0);
    gpio_control(GPIO9, IOT_GPIO_VALUE0);
    gpio_control(GPIO10, IOT_GPIO_VALUE0);
}


// 小车停止
// GPIO0和1为高电平,左轮刹车
// GPIO9和10为高电平,右边轮刹车
void car_stop(void) {
    car_dir_index = DIR_STOP;
    gpio_control(GPIO0, IOT_GPIO_VALUE1);
    gpio_control(GPIO1, IOT_GPIO_VALUE1);
    gpio_control(GPIO9, IOT_GPIO_VALUE1);
    gpio_control(GPIO10, IOT_GPIO_VALUE1);
}